import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from roboticstoolbox import *
import numpy as np
from kinematic import *
def linearPlanning(p1f, p2f, dotNum,patience=0.5):
    '''
    *@brief:   笛卡尔空间直线规划函数
    *@date:    2021.9.12
    *@param:   p1f：起始点坐标，元组形式输入
    *@param:   p2f：终止点坐标，元组形式输入
    *@param:   dotNum：插值点数，以int形式输入
    *@returnParam:   pointf：规划后的点坐标list
    '''
    if not (isinstance(p1f, list) and isinstance(p2f, list)):
        print('两个路径点需要以列表的形式输入')
        return
    if len(p1f) != 3 or len(p2f) != 3:
        print('p1,p2中存在某点列表长度不为3，请检查')
        return
    pointf = []  # 存储路径点
    
    dis=np.sqrt(sum((np.array(p1f[0:3])-np.array(p2f[0:3]))**2))
    if dis<=0.5:
        pointf.append(p1f)
        pointf.append(p2f)
        return pointf
    
    deltax = (p2f[0] - p1f[0]) / dotNum
    deltay = (p2f[1] - p1f[1]) / dotNum
    deltaz = (p2f[2] - p1f[2]) / dotNum
    
    currentIndex = 0
    while currentIndex <= dotNum:
        x = p1f[0] + deltax * currentIndex
        y = p1f[1] + deltay * currentIndex
        z = p1f[2] + deltaz * currentIndex
        pointf.append([x, y, z])
        currentIndex += 1
    return pointf

def joint_planning(via,dt,tacc,qdmax=None,tsegment=None):
    if qdmax is not None:
        traj = mstraj(via, dt=dt, tacc=tacc, qdmax=qdmax)
    else:
        traj = mstraj(via, dt=dt, tacc=tacc, tsegment=tsegment)
    return traj.q

def cartesian_planning(via,dt,tacc,qdmax):
    """笛卡尔多段轨迹规划，多项式融合方案

    Args:
        via (_type_): 经过点
        dt (_type_): 采样时间间隔
        tacc (_type_): 各段加速时间
        qdmax (_type_): 各段的最大速度，即线性处速度

    Returns:
        _type_: 空间轨迹
    """
    traj = mstraj(via, dt=dt, tacc=tacc, qdmax=qdmax)
    return traj.q
def joint_planning_test():
    # 串口设置
    ser = serial.Serial('COM3', 115200, timeout=0.1)
    servo_reset(ser)  
    joints=np.array([[0,90,0,0,0],[0,0,0,0,0]])
    while True:
        # 从串口读取数据
        data = ser.readline().decode('utf-8').strip()
        
        if data:
            print(f"Received data: {data}")

        # 发送数据到串口
        user_input = input("Enter target (j1-j5): ")
        
        if user_input:
            # 提取坐标
            
            joints[1] = extract_joints(user_input)
            # print(cartesian)
            joints_traj=joint_planning(joints)
            # print(joints_traj)

            for i in range(len(joints_traj)):
                send_command(ser,np.array(joints_traj[i]),time='0050')                
                
            joints_traj[0]=joints_traj[1]
        
        else:
            break
    
    
    
    
def cartesian_planning_test():
    # 串口设置
    ser=UART()
    ser.port='COM3'
    ser.baudrate=115200
    ser.open_port()
    # servo_reset(ser)  
    cartesian=np.array([[0,0,350,0,0],[0,0,0,0,0]])
    while True:
    

        # 发送数据到串口
        user_input = input("Enter target (x, y, z, roll, pitch): ")
        
        if user_input:
            # 提取坐标
            
            cartesian[1] = extract_cartesian(user_input)
            # print(cartesian)
            cartesian_traj=cartesian_planning(cartesian)
            # print(cartesian_traj)
            joint_angles=[]
            all_can_inverse=True
            for i in range(len(cartesian_traj)):
                ret,_joint_angles,best_alpha=inverse_kinematic(cartesian_traj[i,0],cartesian_traj[i,1],cartesian_traj[i,2],cartesian_traj[i,3],cartesian_traj[i,4])
                if ret:
                    joint_angles.append(_joint_angles)
                    # send_command(ser,np.array(joint_angles[0]))                
                else:
                    print("逆解求解失败，取消执行！")
                    all_can_inverse=False
                    break;
            if all_can_inverse:
                print(joint_angles[-1])
                for i in range(len(joint_angles)):
                    ser.send_command(np.array(joint_angles[i][0]),time='0100')
                
            cartesian[0]=cartesian[1]
        
        else:
            break

def linear_planning_test():
    # 串口设置
    ser = serial.Serial('COM3', 115200, timeout=0.1)
    servo_reset(ser)  
    coordinate=np.array([[0,0,350],[0,0,0]])
    while True:
        # 从串口读取数据
        data = ser.readline().decode('utf-8').strip()
        
        if data:
            print(f"Received data: {data}")

        # 发送数据到串口
        user_input = input("Enter target (x, y, z): ")
        
        if user_input:
            # 提取坐标
            
            coordinate[1] = extract_coordinate(user_input)
            # print(cartesian)
            coordinate_traj=np.array(linearPlanning(list(coordinate[0]),list(coordinate[1]),dotNum=50))
            print(coordinate_traj)
            joint_angles=[]
            all_can_inverse=True
            for i in range(len(coordinate_traj)):
                ret,_joint_angles=inverse_kinematic(coordinate_traj[i,0],coordinate_traj[i,1],coordinate_traj[i,2],0,0)
                if ret:
                    joint_angles.append(_joint_angles)
                    # send_command(ser,np.array(joint_angles[0]))                
                else:
                    print("逆解求解失败，取消执行！")
                    all_can_inverse=False
                    break;
            if all_can_inverse:
                for i in range(len(joint_angles)):
                    send_command(ser,np.array(joint_angles[i][0]),time='0050')
                
            coordinate[0]=coordinate[1]
        
        else:
            break
        
# cartesian_planning_test()